#include "road_marking_detection.h"
#include "calculate_tools.h"
#include "debug_show.h"
#include "fit.h"
#include "least_square_fitting.h"

namespace rmd {

RoadMarkingDetection::RoadMarkingDetection() {}

RoadMarkingDetection::~RoadMarkingDetection() {}

void RoadMarkingDetection::Init(std::string configFilePath, int width,
                                int height) {
  this->data_frame = new DataFrame();
  this->data_frame->Init(configFilePath, width, height);
  lane_detection = new LaneDetection();

  output_imshow_result.sDepartureOrientation = 0;
  departure_warning = 0;
  mLdwMinDis = 30;
  mLdwThreadK = 0.80;
}

std::string RoadMarkingDetection::GetRoadMarkDetectionVersion() {
  std::string strVersion = "LaneVersion:";
#ifdef _DEBUG
  strVersion += "debug,";
#else
  strVersion += "release,";
#endif

  strVersion += "pulian_ldw_v2.1.0";

  return strVersion;
}

bool RoadMarkingDetection::LimitingCondition(int x, int y, short limit_type) {
  bool res_flag = false;
  switch (limit_type) {
    case kInputLimit: {
      if (x > kImageWidth / 4 && x < 3 * kImageWidth / 4 &&
          y > kImageHeight / 4 &&
          y < std::min(3 * kImageHeight / 4,
                       kImageHeight - 2 * kRoiHeight / 3)) {
        res_flag = true;
      }
      break;
    }
    default:
      break;
  }
  return res_flag;
}

void RoadMarkingDetection::SetBelowSpeedStatus() {
  CKalman::Instance()->EkfInitialization();
  data_frame->pri_refine_results.clear();
  //  data_frame->auto_calibration_flag = true;
  data_frame->auto_frame_index = 0;
  data_frame->auto_ld_vps.clear();
  data_frame->auto_select_ld_vps.clear();

  for (int i = 0; i < 4; i++) {
    data_frame->virtaul_point[i] = Point(0, 0);
  }
}

void RoadMarkingDetection::setVanishPoint(float x, float y, bool bflag) {
  int temp_x = x;
  int temp_y = y;
  if (bflag) {
    temp_x = x * (kImageWidth / (1.0 * kSrcImageWidth));
    temp_y = y * (kImageHeight / (1.0 * kSrcImageHeight));
  }

  //(1)input limit
  bool check_flag = LimitingCondition(temp_x, temp_y, kInputLimit);
  if (check_flag) {
    if (bflag) {
      data_frame->user_vanishing_point = cv::Point(temp_x, temp_y);
      data_frame->error_code = false;
    }
  } else {
    data_frame->error_code = true;
    return;
  }

  if (data_frame->UpdateInit(temp_x, temp_y, bflag))
    data_frame->auto_calibration_flag = true;
}

void RoadMarkingDetection::setLdwParameter(short _ldwMinDis, float _ldwThreadK,
                                           short update_gap,
                                           short auto_frame_gap,
                                           short stable_num) {
  data_frame->Update_Gap = update_gap;
  data_frame->Auto_Frame_Gap = auto_frame_gap;
  data_frame->Stable_Num = stable_num;
  mLdwMinDis = _ldwMinDis;
  mLdwThreadK = _ldwThreadK;
}

void RoadMarkingDetection::RelocationZoneByVP(int _vanishY) {
  Point Pt0(0, 0), Pt1(0, 0), Pt2(0, 0), Pt3(0, 0), Pt4(0, 0), Pt5(0, 0);
  Pt0.x = mLeftPoints[0].x;
  Pt0.y = mLeftPoints[0].y;
  Pt1.x = mLeftPoints[1].x;
  Pt1.y = mLeftPoints[1].y;
  Pt2.x = mLeftPoints[2].x;
  Pt2.y = mLeftPoints[2].y;
  Pt3.x = mRightPoints[0].x;
  Pt3.y = mRightPoints[0].y;
  Pt4.x = mRightPoints[1].x;
  Pt4.y = mRightPoints[1].y;
  Pt5.x = mRightPoints[2].x;
  Pt5.y = mRightPoints[2].y;

  Point2f pt = CalcuateTools::GetInstance()->GetCrossPoint(Pt0, Pt1, Pt4, Pt5);
  int tVanishY = _vanishY > pt.y ? _vanishY : pt.y;

  mSelectPoints[0] = mLineType;

  if (Pt0.y < tVanishY) {
    if (Pt1.y < tVanishY) {
      Point2f retValue_l =
          CalcuateTools::GetInstance()->GetXorYToPt(Pt1, Pt2, tVanishY);
      Point2f retValue_r =
          CalcuateTools::GetInstance()->GetXorYToPt(Pt3, Pt4, tVanishY);
      mSelectPoints[1] = retValue_l;
      mSelectPoints[2] = retValue_l;
      mSelectPoints[3] = Pt2;
      mSelectPoints[4] = Pt3;
      mSelectPoints[5] = retValue_r;
      mSelectPoints[6] = retValue_r;
    } else {
      Point2f retValue_l =
          CalcuateTools::GetInstance()->GetXorYToPt(Pt1, Pt0, tVanishY);
      Point2f retValue_r =
          CalcuateTools::GetInstance()->GetXorYToPt(Pt4, Pt5, tVanishY);
      mSelectPoints[1] = retValue_l;
      mSelectPoints[2] = Pt1;
      mSelectPoints[3] = Pt2;
      mSelectPoints[4] = Pt3;
      mSelectPoints[5] = Pt4;
      mSelectPoints[6] = retValue_r;
    }
  } else {
    mSelectPoints[1] = Pt0;
    mSelectPoints[2] = Pt1;
    mSelectPoints[3] = Pt2;
    mSelectPoints[4] = Pt3;
    mSelectPoints[5] = Pt4;
    mSelectPoints[6] = Pt5;
  }
}

Json::Value RoadMarkingDetection::createResultJson(CALMCARLDWDATA& _vecResult,
                                                   double _time) {
  float fk_x = kSrcImageWidth / (1.0 * kImageWidth);
  float fk_y = kSrcImageHeight / (1.0 * kImageHeight);
  int _vany0 = data_frame->constant_vanishing_point.x;
  int _vany1 = data_frame->constant_vanishing_point.y;
  short ldwOrientation = _vecResult.sDepartureOrientation;
  short ldwDetectionResult = _vecResult.sLaneDetectionResult;

  Json::Value root;
  root["time"] = _time;
  root["vpy0"] = (int)(_vany0 * fk_x);
  root["vpy1"] = (int)(_vany1 * fk_y);
  root["sDepartureOrientation"] = ldwOrientation;
  root["sLaneDetectionResult"] = ldwDetectionResult;

  if (ldwOrientation != 0) {
    short sLeftValue = 0;
    short sCurrentValue = 0;
    short sRightValue = 0;
    CalcuateTools::GetInstance()->GetBinaryValue(ldwDetectionResult, sLeftValue,
                                                 sCurrentValue, sRightValue);

    Json::Value jvirtaulPoint;
    for (int i = 0; i < 3; i++) {
      Point laneResult = _vecResult.virtaulPoint[i];
      Json::Value jVPoint;
      jVPoint["x"] = (int)(laneResult.x * fk_x);
      jVPoint["y"] = (int)(laneResult.y * fk_y);
      jvirtaulPoint.append(jVPoint);
    }
    root["virtaulPoint"] = jvirtaulPoint;

    if (sLeftValue) {
      Json::Value jleftZone;
      for (int i = 0; i < 7; i++) {
        Point laneResult = _vecResult.leftZone[i];
        Json::Value jVPoint;
        jVPoint["x"] = (int)(laneResult.x * fk_x);
        jVPoint["y"] = (int)(laneResult.y * fk_y);
        jleftZone.append(jVPoint);
      }
      root["leftZone"] = jleftZone;
    }

    if (sCurrentValue) {
      Json::Value jcurrentZone;
      for (int i = 0; i < 7; i++) {
        Point laneResult = _vecResult.currentZone[i];
        Json::Value jVPoint;
        jVPoint["x"] = (int)(laneResult.x * fk_x);
        jVPoint["y"] = (int)(laneResult.y * fk_y);
        jcurrentZone.append(jVPoint);
      }
      root["currentZone"] = jcurrentZone;
    }

    if (sRightValue) {
      Json::Value jrightZone;
      for (int i = 0; i < 7; i++) {
        Point laneResult = _vecResult.rightZone[i];
        Json::Value jVPoint;
        jVPoint["x"] = (int)(laneResult.x * fk_x);
        jVPoint["y"] = (int)(laneResult.y * fk_y);
        jrightZone.append(jVPoint);
      }
      root["rightZone"] = jrightZone;
    }
  }

  //  std::string out = root.toStyledString();
  //  return out;
  return root;
}

CALMCARLDWDATA RoadMarkingDetection::OutputAllProcessResult(
    std::vector<LaneResult>& _inResPriSaveLine) {
  if (departure_warning > 0) {
    departure_warning--;
    return mLaneResult;
  }

  CALMCARLDWDATA vecLaneResult;
  vecLaneResult.sDepartureOrientation = 0;
  vecLaneResult.sLaneDetectionResult = 0;

  if (_inResPriSaveLine.size() < 1) {
    data_frame->virtaul_point[0].x = 0;
    if (data_frame->virtaul_point[0].x == 1)
      data_frame->virtaul_point[0].y = 0;
    else if (data_frame->virtaul_point[0].x >= 10)
      data_frame->virtaul_point[0].y = 10;
    else
      data_frame->virtaul_point[0].y++;
    return vecLaneResult;
  }

  short bLeft = 0;
  short bCurrent = 0;
  short bRight = 0;

  LaneResult singleFitlaneResult[4];
  for (int i = 0; i < 4; i++) {
    singleFitlaneResult[i].bIsTrue = false;
  }

  for (int i = 0; i < _inResPriSaveLine.size(); i++) {
    switch (_inResPriSaveLine[i].nIsCurLane) {
      case (kCurrentLineIndexLeft - 1):
        singleFitlaneResult[0] = _inResPriSaveLine[i];
        singleFitlaneResult[0].bIsTrue = true;
        break;
      case kCurrentLineIndexLeft:
        singleFitlaneResult[1] = _inResPriSaveLine[i];
        singleFitlaneResult[1].bIsTrue = true;
        break;
      case kCurrentLineIndexRight:
        singleFitlaneResult[2] = _inResPriSaveLine[i];
        singleFitlaneResult[2].bIsTrue = true;
        break;
      case (kCurrentLineIndexRight + 1):
        singleFitlaneResult[3] = _inResPriSaveLine[i];
        singleFitlaneResult[3].bIsTrue = true;
        bRight = 1;
        break;
      default:
        break;
    }
  }

  if (singleFitlaneResult[1].bIsTrue && singleFitlaneResult[2].bIsTrue) {
    bCurrent = 1;
    float nMinLaneWidth =
        abs(singleFitlaneResult[1].pCenter - singleFitlaneResult[2].pCenter);
    if (data_frame->pri_lane_result.sIsUse > 0 &&
        abs(data_frame->refine_lane_width - nMinLaneWidth) >
            data_frame->refine_lane_width / 3) {
      data_frame->pri_lane_result.sIsUse--;
      return mLaneResult;
    }

    // save data_frame->pri_lane_result
    data_frame->pri_lane_result.leftBottomPoint =
        singleFitlaneResult[1].Line_now[1];
    data_frame->pri_lane_result.leftCenterPoint =
        singleFitlaneResult[1].Line_now[0];
    data_frame->pri_lane_result.leftBottomPoint =
        singleFitlaneResult[1].Line_now[2];
    data_frame->pri_lane_result.rightTopPoint =
        singleFitlaneResult[2].Line_now[1];
    data_frame->pri_lane_result.rightCenterPoint =
        singleFitlaneResult[2].Line_now[0];
    data_frame->pri_lane_result.rightBottomPoint =
        singleFitlaneResult[2].Line_now[2];

    short tDepartureOrientation = 1;

    //     if (abs(gMinLaneWidth - nMinLaneWidth) < gMinLaneWidth / 2)
    if (data_frame->pri_lane_result.sIsUse >= kStableNum / 2 &&
        output_imshow_result.sDepartureOrientation > 0) {
      if (singleFitlaneResult[1].pCenter > 2 * mLdwMinDis) {
        tDepartureOrientation = 2;
      }
      if (singleFitlaneResult[2].pCenter < -2 * mLdwMinDis) {
        if (tDepartureOrientation == 2) {
          tDepartureOrientation =
              (singleFitlaneResult[1].pCenter > -singleFitlaneResult[2].pCenter)
                  ? 2
                  : 3;
        } else {
          tDepartureOrientation = 3;
        }
      }
      if (tDepartureOrientation == 1) {
        Point2f curVanishPt = CalcuateTools::GetInstance()->GetCrossPoint(
            singleFitlaneResult[1].Line_now[0],
            singleFitlaneResult[1].Line_now[1],
            singleFitlaneResult[2].Line_now[0],
            singleFitlaneResult[2].Line_now[1]);

        float fSingleK = abs(singleFitlaneResult[1].pCenter + kImageWidth / 2 -
                             curVanishPt.x) /
                         (1.0 * abs(singleFitlaneResult[2].pCenter +
                                    kImageWidth / 2 - curVanishPt.x));
        if (fSingleK < 1 - 0.9 * mLdwThreadK) {
          tDepartureOrientation = 2;
        } else if (fSingleK > 1 + mLdwThreadK) {
          tDepartureOrientation = 3;
        }
      }
    }

    vecLaneResult.sDepartureOrientation = tDepartureOrientation;

    if (tDepartureOrientation > 1) {
      departure_warning = 3;
    }

    // vanish point control
    mLineType.x = singleFitlaneResult[1].nLineType;
    mLineType.y = singleFitlaneResult[2].nLineType;
    mLeftPoints[0].x = singleFitlaneResult[1].Line_now[2].x;
    mLeftPoints[0].y = singleFitlaneResult[1].Line_now[2].y;
    mLeftPoints[1].x = singleFitlaneResult[1].Line_now[0].x;
    mLeftPoints[1].y = singleFitlaneResult[1].Line_now[0].y;
    mLeftPoints[2].x = singleFitlaneResult[1].Line_now[1].x;
    mLeftPoints[2].y = singleFitlaneResult[1].Line_now[1].y;
    mRightPoints[0].x = singleFitlaneResult[2].Line_now[1].x;
    mRightPoints[0].y = singleFitlaneResult[2].Line_now[1].y;
    mRightPoints[1].x = singleFitlaneResult[2].Line_now[0].x;
    mRightPoints[1].y = singleFitlaneResult[2].Line_now[0].y;
    mRightPoints[2].x = singleFitlaneResult[2].Line_now[2].x;
    mRightPoints[2].y = singleFitlaneResult[2].Line_now[2].y;

    RelocationZoneByVP(data_frame->adapt_vanishing_point.y);

    memcpy(vecLaneResult.currentZone, mSelectPoints, sizeof(mSelectPoints));
    // vecLaneResult.currentZone = mSelectPoints;

    data_frame->virtaul_point[1].x = (singleFitlaneResult[1].Line_now[2].x +
                                      singleFitlaneResult[2].Line_now[2].x) /
                                     2;
    data_frame->virtaul_point[1].y = singleFitlaneResult[2].Line_now[2].y;
    data_frame->virtaul_point[2].x = (singleFitlaneResult[1].Line_now[0].x +
                                      singleFitlaneResult[2].Line_now[0].x) /
                                     2;
    data_frame->virtaul_point[2].y = singleFitlaneResult[2].Line_now[0].y;
    data_frame->virtaul_point[3].x = (singleFitlaneResult[1].Line_now[1].x +
                                      singleFitlaneResult[2].Line_now[1].x) /
                                     2;
    data_frame->virtaul_point[3].y = singleFitlaneResult[2].Line_now[1].y;

    data_frame->virtaul_point[0].x = 1;
    if (data_frame->virtaul_point[0].x == 0)
      data_frame->virtaul_point[0].y = 1;
    else if (data_frame->virtaul_point[0].x >= 10)
      data_frame->virtaul_point[0].y = 10;
    else
      data_frame->virtaul_point[0].y++;
  } else if (singleFitlaneResult[1].bIsTrue) {
    if (!bCurrent) {
      if (singleFitlaneResult[1].pCenter > 0 &&
          singleFitlaneResult[1].pCenter > mLdwMinDis) {
        vecLaneResult.sDepartureOrientation = 2;
      } else if (data_frame->virtaul_point[0].x ==
                 1 /* && data_frame->virtaul_point[0].y >= 10*/) {
        // visual center line
        mLineType.x = singleFitlaneResult[1].nLineType;
        mLineType.y = singleFitlaneResult[2].nLineType;
        mLeftPoints[0].x = singleFitlaneResult[1].Line_now[2].x;
        mLeftPoints[0].y = singleFitlaneResult[1].Line_now[2].y;
        mLeftPoints[1].x = singleFitlaneResult[1].Line_now[0].x;
        mLeftPoints[1].y = singleFitlaneResult[1].Line_now[0].y;
        mLeftPoints[2].x = singleFitlaneResult[1].Line_now[1].x;
        mLeftPoints[2].y = singleFitlaneResult[1].Line_now[1].y;
        mRightPoints[0].x = 2 * data_frame->virtaul_point[3].x -
                            singleFitlaneResult[1].Line_now[1].x;
        mRightPoints[0].y = singleFitlaneResult[1].Line_now[1].y;
        mRightPoints[1].x = 2 * data_frame->virtaul_point[2].x -
                            singleFitlaneResult[1].Line_now[0].x;
        mRightPoints[1].y = singleFitlaneResult[1].Line_now[0].y;
        mRightPoints[2].x = 2 * data_frame->virtaul_point[1].x -
                            singleFitlaneResult[1].Line_now[2].x;
        mRightPoints[2].y = singleFitlaneResult[1].Line_now[2].y;

        RelocationZoneByVP(data_frame->adapt_vanishing_point.y);
        //                    vecLaneResult.currentZone = mSelectPoints;
        memcpy(vecLaneResult.currentZone, mSelectPoints, sizeof(mSelectPoints));
      }
    }

    if (singleFitlaneResult[0].bIsTrue) {
      bLeft = 1;
      mLineType.x = singleFitlaneResult[0].nLineType;
      mLineType.y = singleFitlaneResult[1].nLineType;
      mLeftPoints[0].x = singleFitlaneResult[0].Line_now[2].x;
      mLeftPoints[0].y = singleFitlaneResult[0].Line_now[2].y;
      mLeftPoints[1].x = singleFitlaneResult[0].Line_now[0].x;
      mLeftPoints[1].y = singleFitlaneResult[0].Line_now[0].y;
      mLeftPoints[2].x = singleFitlaneResult[0].Line_now[1].x;
      mLeftPoints[2].y = singleFitlaneResult[0].Line_now[1].y;
      mRightPoints[0].x = singleFitlaneResult[1].Line_now[1].x;
      mRightPoints[0].y = singleFitlaneResult[1].Line_now[1].y;
      mRightPoints[1].x = singleFitlaneResult[1].Line_now[0].x;
      mRightPoints[1].y = singleFitlaneResult[1].Line_now[0].y;
      mRightPoints[2].x = singleFitlaneResult[1].Line_now[2].x;
      mRightPoints[2].y = singleFitlaneResult[1].Line_now[2].y;

      RelocationZoneByVP(data_frame->adapt_vanishing_point.y);
      //                vecLaneResult.leftZone = mSelectPoints;
      memcpy(vecLaneResult.leftZone, mSelectPoints, sizeof(mSelectPoints));
    }
  } else if (singleFitlaneResult[2].bIsTrue) {
    if (!bCurrent) {
      if (singleFitlaneResult[2].pCenter < 0 &&
          abs(singleFitlaneResult[2].pCenter) > mLdwMinDis)
        vecLaneResult.sDepartureOrientation = 3;
      else if (data_frame->virtaul_point[0].x ==
               1 /* && data_frame->virtaul_point[0].y >= 10*/) {
        // visual center line
        mLineType.x = singleFitlaneResult[1].nLineType;
        mLineType.y = singleFitlaneResult[2].nLineType;
        mLeftPoints[0].x = singleFitlaneResult[2].Line_now[2].x;
        mLeftPoints[0].y = singleFitlaneResult[2].Line_now[2].y;
        mLeftPoints[1].x = singleFitlaneResult[2].Line_now[0].x;
        mLeftPoints[1].y = singleFitlaneResult[2].Line_now[0].x;
        mLeftPoints[2].x = singleFitlaneResult[2].Line_now[1].x;
        mLeftPoints[2].y = singleFitlaneResult[2].Line_now[1].x;
        mRightPoints[0].x = 2 * data_frame->virtaul_point[1].x -
                            singleFitlaneResult[2].Line_now[1].x;
        mRightPoints[0].y = singleFitlaneResult[2].Line_now[1].y;
        mRightPoints[1].x = 2 * data_frame->virtaul_point[2].x -
                            singleFitlaneResult[2].Line_now[0].x;
        mRightPoints[1].y = singleFitlaneResult[2].Line_now[0].y;
        mRightPoints[2].x = 2 * data_frame->virtaul_point[3].x -
                            singleFitlaneResult[2].Line_now[2].x;
        mRightPoints[2].y = singleFitlaneResult[2].Line_now[2].y;

        RelocationZoneByVP(data_frame->adapt_vanishing_point.y);
        //                    vecLaneResult.currentZone = mSelectPoints;
        memcpy(vecLaneResult.currentZone, mSelectPoints, sizeof(mSelectPoints));
      }
    }

    if (singleFitlaneResult[3].bIsTrue) {
      bRight = 1;

      mLineType.x = singleFitlaneResult[2].nLineType;
      mLineType.y = singleFitlaneResult[3].nLineType;
      mLeftPoints[0].x = singleFitlaneResult[2].Line_now[2].x;
      mLeftPoints[0].y = singleFitlaneResult[2].Line_now[2].y;
      mLeftPoints[1].x = singleFitlaneResult[2].Line_now[0].x;
      mLeftPoints[1].y = singleFitlaneResult[2].Line_now[0].y;
      mLeftPoints[2].x = singleFitlaneResult[2].Line_now[1].x;
      mLeftPoints[2].y = singleFitlaneResult[2].Line_now[1].y;
      mRightPoints[0].x = singleFitlaneResult[3].Line_now[1].x;
      mRightPoints[0].y = singleFitlaneResult[3].Line_now[1].y;
      mRightPoints[1].x = singleFitlaneResult[3].Line_now[0].x;
      mRightPoints[1].y = singleFitlaneResult[3].Line_now[0].y;
      mRightPoints[2].x = singleFitlaneResult[3].Line_now[2].x;
      mRightPoints[2].y = singleFitlaneResult[3].Line_now[2].y;

      RelocationZoneByVP(data_frame->adapt_vanishing_point.y);
      memcpy(vecLaneResult.rightZone, mSelectPoints, sizeof(mSelectPoints));
    }
  }

  vecLaneResult.sLaneDetectionResult =
      CalcuateTools::GetInstance()->GetInt(bLeft, bCurrent, bRight);

  vecLaneResult.virtaulPoint[0] = data_frame->virtaul_point[1];
  vecLaneResult.virtaulPoint[1] = data_frame->virtaul_point[2];
  vecLaneResult.virtaulPoint[2] = data_frame->virtaul_point[3];

  return vecLaneResult;
}

CALMCARLDWDATA RoadMarkingDetection::CheckShowResult(
    CALMCARLDWDATA& _inLaneResult) {
  CALMCARLDWDATA resLaneResult;
  short sIsGood = -1;

  if (_inLaneResult.sDepartureOrientation == 0 &&
      data_frame->pri_lane_result.sIsUse > 0) {
    sIsGood = 1;
  } else {
    sIsGood = 0;

    short sLeftValue = 0;
    short sCurrentValue = 0;
    short sRightValue = 0;
    CalcuateTools::GetInstance()->GetBinaryValue(
        _inLaneResult.sLaneDetectionResult, sLeftValue, sCurrentValue,
        sRightValue);
    if (sCurrentValue) {
      Point pt1 = _inLaneResult.currentZone[1];
      Point pt2 = _inLaneResult.currentZone[2];
      Point pt3 = _inLaneResult.currentZone[3];
      Point pt4 = _inLaneResult.currentZone[4];
      Point pt5 = _inLaneResult.currentZone[5];
      Point pt6 = _inLaneResult.currentZone[6];

      float frhoL = CalcuateTools::GetInstance()->CalRho(pt2, pt3);
      float frhoR = CalcuateTools::GetInstance()->CalRho(pt5, pt4);
      //               cout<<"fRHO:"<<frhoL<<","<<frhoR<<endl;
      //      int left_self_dis = abs(pt1.x - pt3.x);
      //      int lright_self_dis = abs(pt4.x - pt6.x);

      int top_dis = pt6.x - pt1.x;
      int bottom_dis = pt4.x - pt3.x;
      int center_dis = pt5.x - pt2.x;

      if (top_dis > 10 &&
          center_dis >
              std::min((int)data_frame->refine_lane_width / 2, top_dis) &&
          bottom_dis >
              std::max(kMinWidthScale * kImageWidth, 1.5f * center_dis) &&
          bottom_dis <
              std::min(3.0f * center_dis, kMaxWidthScale * kImageWidth) &&
          !(abs(abs(frhoL) - 90) <= kControlMinRho &&
            abs(abs(frhoR) - 90) <= kControlMinRho)) {
        data_frame->pri_lane_result.sIsUse = kStableNum;
        sIsGood = 2;
        data_frame->show_left_rho = frhoL;
        data_frame->show_right_rho = frhoR;
        //        lane_detection->removeEkfSaveLinesByCurrent(_inLaneResult.left_index,
        //                                                    _inLaneResult.right_index);
      }
    }
  }

  switch (sIsGood) {
    case 0:
      if (data_frame->pri_lane_result.sIsUse > 0) {
        resLaneResult = mLaneResult;
        data_frame->pri_lane_result.sIsUse--;
      }
      //           gPriLaneResult.sIsUse = false;
      break;
    case 1:
      resLaneResult = mLaneResult;
      data_frame->pri_lane_result.sIsUse = 0;
      break;
    case 2:
      resLaneResult = _inLaneResult;
      mLaneResult = _inLaneResult;
      break;
    default:
      break;
  }

  //  std::cout << "sIsGood:" << sIsGood << std::endl;

  return resLaneResult;
}

void RoadMarkingDetection::GetInitRoiZone(CALMCARLDWDATA& _vecResult) {
  if (_vecResult.sDepartureOrientation == -1) {
    int nDisVp = 20;
    int theight = kRoiHeight;

    Point vanPt;
    vanPt.x = data_frame->constant_vanishing_point.x;
    vanPt.y = data_frame->constant_vanishing_point.y;

    int tEndLine = vanPt.y + theight;  // 2*nDisVp
    int tMidFileStartLine = vanPt.y + nDisVp;
    int tStartLine =
        (data_frame->end_line + 2 * data_frame->middle_start_line) / 3;

    int tBottomH = rmd::max(0, kImageWidth / 2 - theight);
    Point pBottom = Point(tBottomH, tEndLine);
    _vecResult.currentZone[3] = pBottom;
    Point2f resPt =
        CalcuateTools::GetInstance()->GetXorYToPt(pBottom, vanPt, tStartLine);
    _vecResult.currentZone[2] = resPt;
    resPt = CalcuateTools::GetInstance()->GetXorYToPt(
        pBottom, vanPt, rmd::max(tMidFileStartLine, vanPt.y + nDisVp));
    _vecResult.currentZone[1] = resPt;

    tBottomH = rmd::min((int)kImageWidth, kImageWidth / 2 + theight);
    pBottom = Point(tBottomH, tEndLine);
    _vecResult.currentZone[4] = pBottom;
    resPt =
        CalcuateTools::GetInstance()->GetXorYToPt(pBottom, vanPt, tStartLine);
    _vecResult.currentZone[5] = resPt;
    resPt = CalcuateTools::GetInstance()->GetXorYToPt(
        pBottom, vanPt, rmd::max(tMidFileStartLine, vanPt.y + nDisVp));
    _vecResult.currentZone[6] = resPt;

    _vecResult.sLaneDetectionResult = 2;
  }
}

void RoadMarkingDetection::setInputImage(cv::Mat input_image) {
  this->data_frame->process_image = input_image;
}

CALMCARLDWDATA RoadMarkingDetection::Update(cv::Mat image) {
  CALMCARLDWDATA tLaneResult;
  tLaneResult.sDepartureOrientation = 0;
  tLaneResult.sLaneDetectionResult = 0;
  this->data_frame->process_grayimage = image;

  //  cv::imshow("process_grayimage", this->data_frame->process_grayimage);

  if (!data_frame->error_code) {
    std::vector<LaneResult> lane_line_results =
        lane_detection->Update(this->data_frame, mVehicleInfo);

    if (data_frame->auto_calibration_flag) {
      data_frame->auto_frame_index++;
      tLaneResult.sDepartureOrientation = -1;
    } else {
      tLaneResult = OutputAllProcessResult(lane_line_results);

      // check show figure
      tLaneResult = CheckShowResult(tLaneResult);

      if (lane_line_results.size() > 1) {
        data_frame->miss_ldw_cout = 0;
        data_frame->good_ldw_cout =
            std::min(++data_frame->good_ldw_cout,
                     (int)data_frame->Auto_Frame_Gap * data_frame->Update_Gap);
      } else {
        data_frame->miss_ldw_cout++;
        if (data_frame->good_ldw_cout > data_frame->Update_Gap) {
          data_frame->good_ldw_cout--;
        } else
          data_frame->good_ldw_cout = 0;
        if (data_frame->good_ldw_cout == 0 &&
            data_frame->miss_ldw_cout >
                data_frame->Auto_Frame_Gap * data_frame->Update_Gap) {
          data_frame->auto_calibration_flag = true;
          setVanishPoint(data_frame->user_vanishing_point.x,
                         data_frame->user_vanishing_point.y, false);
          data_frame->miss_ldw_cout = 0;
        }
      }
      data_frame->auto_frame_index = 0;
    }

    data_frame->pri_refine_results = lane_line_results;

  } else {
    tLaneResult.sDepartureOrientation = -1;
    data_frame->auto_frame_index++;
    if (data_frame->auto_frame_index >
        data_frame->Auto_Frame_Gap * data_frame->Update_Gap) {
      setVanishPoint(data_frame->user_vanishing_point.x,
                     data_frame->user_vanishing_point.y, false);
      data_frame->auto_calibration_flag = true;
      data_frame->auto_frame_index = 0;
      data_frame->auto_ld_vps.clear();
    }
    return tLaneResult;
  }

  GetInitRoiZone(tLaneResult);
  //  std::cout << "auto_frame_index:" << data_frame->auto_frame_index <<
  //  std::endl;

  output_imshow_result = tLaneResult;

  return tLaneResult;
}
}
